#include <boost/program_options.hpp>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/point_operators.h>
#include <pcl/common/io.h>
#include <pcl/search/organized.h>
#include <pcl/search/octree.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/surface/gp3.h>
#include <pcl/io/vtk_io.h>
#include <pcl/filters/voxel_grid.h>
 
#include <iostream>
#include <fstream>
 
#include <string>
#include <vector>
#include <sstream>

using namespace pcl;
using namespace std;
 
namespace po = boost::program_options;

 //参考：https://blog.csdn.net/LIU2089735320/article/details/89363899

struct PXYZI
{
	float x=0;
	float y=0;
	float z=0;
	float intensity=0;
};



vector<string>  GetFileNames(string path,string extend_name)
{
	vector<string> filenames;
	int count =0;
    DIR *pDir;
    struct dirent* ptr;
    if(!(pDir = opendir(path.c_str()))){
        cout<<"Folder doesn't Exist!"<<endl;
        return filenames;
    }
    while((ptr = readdir(pDir))!=0) {
        if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0){
		string sFilename(ptr->d_name);
 		string suffixStr = sFilename.substr(sFilename.find_last_of('.') + 1);//获取文件后缀
	 	if (suffixStr.compare(extend_name) == 0) {//根据后缀筛选文件
            filenames.push_back(path + "/" + ptr->d_name);
			count++;
		 }
    	}
    }

cout<<"共有:"<<count<<"个文件\n";
    closedir(pDir);
	return filenames;
}

void bin2txt(string infile,string outfile)//infile为bin文件全路径，outfile为txt文件全路径
{
		// load point cloud
	fstream input(infile.c_str(), ios::in | ios::binary);
	if(!input.good()){
		cerr << "Could not read file: " << infile << endl;
		exit(EXIT_FAILURE);
	}
	input.seekg(0, ios::beg);
  
   vector<PXYZI>  points_xyzi;
	int i;
	float x_temp;
	float y_temp;
	float z_temp;
	float i_temp;
	for (i=0; input.good() && !input.eof(); i++) {
		PXYZI point;
		float unknowndata;
		input.read((char *) &point.x, sizeof(float));
		input.read((char *) &point.y, sizeof(float));
		input.read((char *) &point.z, sizeof(float));
		input.read((char *) &point.intensity, sizeof(float));

		points_xyzi.push_back(point);
		input.read((char *) &unknowndata, sizeof(float));
	}
	input.close();
 
	cout << "Read  point cloud with " << i << " points, writing to " << outfile << endl;
 
    //  pcl::PCDWriter writer;
 
    // // // Save DoN features
    // writer.write<PointXYZI> (outfile, *points, false);
    ofstream in;
	in.open(outfile,ios::trunc);
	for(int i=0;i<points_xyzi.size();i++)
	{
		in<<points_xyzi[i].x<<" "<<points_xyzi[i].y<<" "<<points_xyzi[i].z<<" "<<points_xyzi[i].intensity<<"\n";
	}
	in.close();
}
void bin2pcd(string infile,string outfile)//infile为bin文件全路径，outfile为pcd文件全路径
{
		// load point cloud
	fstream input(infile.c_str(), ios::in | ios::binary);
	if(!input.good()){
		cerr << "Could not read file: " << infile << endl;
		exit(EXIT_FAILURE);
	}
	input.seekg(0, ios::beg);
 
	pcl::PointCloud<PointXYZI>::Ptr points (new pcl::PointCloud<PointXYZI>);
 
	int i;
	for (i=0; input.good() && !input.eof(); i++) {
		PointXYZI point;
		float unknowndata;
		input.read((char *) &point.x, 3*sizeof(float));
		input.read((char *) &point.intensity, sizeof(float));
		points->push_back(point);
		input.read((char *) &unknowndata, sizeof(float));
	}
	input.close();
 
	cout << "Read  point cloud with " << i << " points, writing to " << outfile << endl;
 
    pcl::PCDWriter writer;
 
    // Save DoN features
    writer.write<PointXYZI> (outfile, *points, false);
}
int main(int argc, char **argv){
	///The file to read from.
	string infile;
 
	///The file to output to.
	string outfile;

	// Declare the supported options.
	po::options_description desc("Program options");
	desc.add_options()
		//Options
		("infile", po::value<string>(&infile)->required(), "the file to read a point cloud from")
		("outfile", po::value<string>(&outfile)->required(), "the file to write the DoN point cloud & normals to")
		;
	// Parse the command line
	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
 
	// Print help
	if (vm.count("help"))
	{
		cout << desc << "\n";
		return false;
	}
 
	// Process options.
	po::notify(vm);
 
	vector<string>  filenames=GetFileNames(infile,"bin");//文件夹中所有文件名



//写pcd文件
string source;
string target;
string suffixStr ;
string outfiletemp=outfile;
for(int i=0;i<filenames.size();i++)
{
	outfile=outfiletemp;
	source=filenames[i];
	suffixStr = source.substr(0,source.find_first_of('.') );//获取文件名称不含后缀
	suffixStr=suffixStr.substr(source.find_last_of("/\\")+1, suffixStr.length());//得到文件名
	target=suffixStr.append(".pcd");
	target=outfile.append(target);
	cout<<"create:"<<target<<"\n";
	bin2pcd(source,target);
//cout<<filenames[i]<<"\n";
}
//写txt文件
for(int i=0;i<filenames.size();i++)
{
	outfile=outfiletemp;
	source=filenames[i];
	suffixStr = source.substr(0,source.find_first_of('.') );//获取文件名称不含后缀
	suffixStr=suffixStr.substr(source.find_last_of("/\\")+1, suffixStr.length());//得到文件名
	target=suffixStr.append(".txt");
	target=outfile.append(target);
	cout<<"create:"<<target<<"\n";
	bin2txt(source,target);
}




	
}

